From ba108534db81798d2284b1b5fb7f8d8566b9aefa Mon Sep 17 00:00:00 2001 From: tsteven4 <13596209+tsteven4@users.noreply.github.com> Date: Mon, 31 Jul 2023 07:26:05 -0600 Subject: [PATCH] Reduce complexity of simplify filter (#1149) * fix bugs with simplify filter 1. With the length option the last point deleted took the total error over the specified limit. 2. When computing the total error if another point is deleted it was possible to refer to an xte record that needed to be updated due to the deletion of one of its neighbors. * wip on reduced complexity simplify filter. * mimic ineheitence of new_trkseg flag in simplify filter. * cleanup simplify header. * work around apparent Qt5 QMap bug. * fix shadowing issues. * astyle * correct spelling. * kill Waypoint route_priority. --- arcdist.cc | 1 - defs.h | 16 +-- route.cc | 19 +++ smplrout.cc | 335 +++++++++++++++++++++++----------------------------- smplrout.h | 58 ++++----- waypt.cc | 3 - 6 files changed, 190 insertions(+), 242 deletions(-) diff --git a/arcdist.cc b/arcdist.cc index b5b541536..483076e95 100644 --- a/arcdist.cc +++ b/arcdist.cc @@ -173,7 +173,6 @@ void ArcDistanceFilter::process() } else if (projectopt) { wp->longitude = ed->prjlongitude; wp->latitude = ed->prjlatitude; - wp->route_priority = 1; if (!arcfileopt && (ed->arcpt2->altitude != unknown_alt) && (ptsopt || (ed->arcpt1->altitude != unknown_alt))) { diff --git a/defs.h b/defs.h index 635f5ae12..d53135a10 100644 --- a/defs.h +++ b/defs.h @@ -448,17 +448,6 @@ public: gpsbabel::DateTime creation_time; wp_flags wpt_flags; - /* - * route priority is for use by the simplify filter. If we have - * some reason to believe that the route point is more important, - * we can give it a higher (numerically; 0 is the lowest) priority. - * This causes it to be removed last. - * This is currently used by the saroute input filter to give named - * waypoints (representing turns) a higher priority. - * This is also used by the google input filter because they were - * nice enough to use exactly the same priority scheme. - */ - int route_priority; /* Optional dilution of precision: positional, horizontal, vertical. * 1 <= dop <= 50 @@ -492,7 +481,6 @@ public: void waypt_del(Waypoint* wpt); // a.k.a. erase() // FIXME: Generally it is inefficient to use an element pointer or reference to define the element to be deleted, use iterator instead, // and/or implement pop_back() a.k.a. removeLast(), and/or pop_front() a.k.a. removeFirst(). - iterator waypt_del(iterator it) {return erase(it);} void del_rte_waypt(Waypoint* wpt); void waypt_compute_bounds(bounds* bounds) const; Waypoint* find_waypt_by_name(const QString& name) const; @@ -525,6 +513,7 @@ public: using QList::front; // a.k.a. first() using QList::rbegin; using QList::rend; + using QList::size_type; }; const global_trait* get_traits(); @@ -674,6 +663,7 @@ public: void copy(RouteList** dst) const; void restore(RouteList* src); void swap(RouteList& other); + void swap_wpts(route_head* rte, WaypointList& other); template void sort(Compare cmp) {std::sort(begin(), end(), cmp);} template @@ -728,6 +718,8 @@ void route_add_wpt(route_head* rte, Waypoint* wpt, QStringView namepart = u"RPT" void track_add_wpt(route_head* rte, Waypoint* wpt, QStringView namepart = u"RPT", int number_digits = 3); void route_del_wpt(route_head* rte, Waypoint* wpt); void track_del_wpt(route_head* rte, Waypoint* wpt); +void route_swap_wpts(route_head* rte, WaypointList& other); +void track_swap_wpts(route_head* rte, WaypointList& other); //void route_disp(const route_head* rte, waypt_cb); /* template */ void route_disp(const route_head* rte, std::nullptr_t /* waypt_cb */); /* override to catch nullptr */ //void route_disp_all(route_hdr, route_trl, waypt_cb); /* template */ diff --git a/route.cc b/route.cc index 95e6940aa..481987226 100644 --- a/route.cc +++ b/route.cc @@ -143,6 +143,18 @@ track_del_wpt(route_head* rte, Waypoint* wpt) global_track_list->del_wpt(rte, wpt); } +void +route_swap_wpts(route_head* rte, WaypointList& other) +{ + global_route_list->swap_wpts(rte, other); +} + +void +track_swap_wpts(route_head* rte, WaypointList& other) +{ + global_track_list->swap_wpts(rte, other); +} + void route_disp(const route_head* /* rh */, std::nullptr_t /* wc */) { @@ -504,3 +516,10 @@ void RouteList::swap(RouteList& other) *this = other; other = tmp_list; } + +void RouteList::swap_wpts(route_head* rte, WaypointList& other) +{ + this->waypt_ct -= rte->rte_waypt_ct(); + this->waypt_ct += other.count(); + rte->waypoint_list.swap(other); +} diff --git a/smplrout.cc b/smplrout.cc index 3ddf224b7..11bc30aa0 100644 --- a/smplrout.cc +++ b/smplrout.cc @@ -1,7 +1,7 @@ /* Route / track simplification filter - Copyright (C) 2002-2014 Robert Lipe, robertlipe+source@gpsbabel.org + Copyright (C) 2002-2023 Robert Lipe, robertlipe+source@gpsbabel.org This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -56,11 +56,12 @@ 2008/08/20: added "relative" option, (Carsten Allefeld, carsten.allefeld@googlemail.com) */ -#include // for sort +#include #include // for strtol -#include // for swap #include // for QDateTime +#include // for QHash +#include // for QMap #include "defs.h" #include "smplrout.h" @@ -71,136 +72,75 @@ #if FILTERS_ENABLED #define MYNAME "simplify" -void SimplifyRouteFilter::free_xte(struct xte* xte_rec) +inline bool operator<(const SimplifyRouteFilter::trackerror& lhs, const SimplifyRouteFilter::trackerror& rhs) { - delete xte_rec->intermed; + return ((lhs.dist > rhs.dist) || ((lhs.dist == rhs.dist) && (lhs.wptpos < rhs.wptpos))); } - -void SimplifyRouteFilter::routesimple_waypt_pr(const Waypoint* wpt) -{ - if (!cur_rte) { - return; - } - xte_recs[xte_count].intermed = new struct xte_intermed; - xte_recs[xte_count].intermed->wpt = wpt; - xte_recs[xte_count].intermed->xte_rec = xte_recs+xte_count; - xte_recs[xte_count].intermed->next = nullptr; - xte_recs[xte_count].intermed->prev = tmpprev; - if (tmpprev) { - tmpprev->next = xte_recs[xte_count].intermed; - } - tmpprev = xte_recs[xte_count].intermed; - xte_count++; -} - -void SimplifyRouteFilter::compute_xte(struct xte* xte_rec) +double SimplifyRouteFilter::compute_track_error(const neighborhood& nb) const { - const Waypoint* wpt3 = xte_rec->intermed->wpt; - double reslat, reslon; /* if no previous, this is an endpoint and must be preserved. */ - if (!xte_rec->intermed->prev) { - xte_rec->distance = kHugeValue; - return; + if (nb.prev == nullptr) { + return kHugeValue; } - const Waypoint* wpt1 = xte_rec->intermed->prev->wpt; + const Waypoint* wpt1 = nb.prev; /* if no next, this is an endpoint and must be preserved. */ - if (!xte_rec->intermed->next) { - xte_rec->distance = kHugeValue; - return; + if (nb.next == nullptr) { + return kHugeValue; } - const Waypoint* wpt2 = xte_rec->intermed->next->wpt; + const Waypoint* wpt2 = nb.next; + const Waypoint* wpt3 = nb.wpt; + double track_error; switch (metric) { case metric_t::crosstrack: - xte_rec->distance = radtomiles(linedist( - wpt1->latitude, wpt1->longitude, - wpt2->latitude, wpt2->longitude, - wpt3->latitude, wpt3->longitude)); + track_error = radtomiles(linedist( + wpt1->latitude, wpt1->longitude, + wpt2->latitude, wpt2->longitude, + wpt3->latitude, wpt3->longitude)); break; case metric_t::length: - xte_rec->distance = radtomiles( - gcdist(wpt1->latitude, wpt1->longitude, - wpt3->latitude, wpt3->longitude) + - gcdist(wpt3->latitude, wpt3->longitude, - wpt2->latitude, wpt2->longitude) - - gcdist(wpt1->latitude, wpt1->longitude, - wpt2->latitude, wpt2->longitude)); + track_error = radtomiles( + gcdist(wpt1->latitude, wpt1->longitude, + wpt3->latitude, wpt3->longitude) + + gcdist(wpt3->latitude, wpt3->longitude, + wpt2->latitude, wpt2->longitude) - + gcdist(wpt1->latitude, wpt1->longitude, + wpt2->latitude, wpt2->longitude)); break; case metric_t::relative: - if (wpt3->hdop == 0) { - fatal(MYNAME ": relative needs hdop information.\n"); - } + default: // eliminate false positive warning with g++ 11.3.0: ‘error’ may be used uninitialized in this function [-Wmaybe-uninitialized] // if timestamps exist, distance to interpolated point - if (wpt1->GetCreationTime().isValid() && + if (wpt1->GetCreationTime().isValid() && wpt2->GetCreationTime().isValid() && wpt3->GetCreationTime().isValid() && (wpt1->GetCreationTime() != wpt2->GetCreationTime())) { double frac = static_cast(wpt1->GetCreationTime().msecsTo(wpt3->GetCreationTime())) / static_cast(wpt1->GetCreationTime().msecsTo(wpt2->GetCreationTime())); + double reslat, reslon; linepart(wpt1->latitude, wpt1->longitude, wpt2->latitude, wpt2->longitude, frac, &reslat, &reslon); - xte_rec->distance = radtometers(gcdist( - wpt3->latitude, wpt3->longitude, - reslat, reslon)); + track_error = radtometers(gcdist( + wpt3->latitude, wpt3->longitude, + reslat, reslon)); } else { // else distance to connecting line - xte_rec->distance = radtometers(linedist( - wpt1->latitude, wpt1->longitude, - wpt2->latitude, wpt2->longitude, - wpt3->latitude, wpt3->longitude)); + track_error = radtometers(linedist( + wpt1->latitude, wpt1->longitude, + wpt2->latitude, wpt2->longitude, + wpt3->latitude, wpt3->longitude)); } // error relative to horizontal precision - xte_rec->distance /= (6 * wpt3->hdop); + track_error /= (6 * wpt3->hdop); // (hdop->meters following to J. Person at ) break; } + return track_error; } -int SimplifyRouteFilter::compare_xte(const void* a, const void* b) -{ - const auto* xte_a = static_cast(a); - const auto* xte_b = static_cast(b); - - if (kHugeValue == xte_a->distance) { - if (kHugeValue == xte_b->distance) { - return 0; - } - return -1; - } - - if (kHugeValue == xte_b->distance) { - return 1; - } - - int priodiff = xte_a->intermed->wpt->route_priority - - xte_b->intermed->wpt->route_priority; - if (priodiff < 0) { - return 1; - } - if (priodiff > 0) { - return -1; - } - - double distdiff = xte_a->distance - xte_b->distance; - if (distdiff < 0) { - return 1; - } - if (distdiff > 0) { - return -1; - } - return 0; -} - -void SimplifyRouteFilter::routesimple_head(const route_head* rte) +void SimplifyRouteFilter::routesimple_tail(const route_head* rte) { - cur_rte = nullptr; - /* build array of XTE/wpt xref records */ - xte_count = 0; - tmpprev = nullptr; - totalerror = 0; - /* short-circuit if we already have fewer than the max points */ if ((limit_basis == limit_basis_t::count) && count >= rte->rte_waypt_ct()) { return; @@ -211,122 +151,139 @@ void SimplifyRouteFilter::routesimple_head(const route_head* rte) return; } - xte_recs = new struct xte[rte->rte_waypt_ct()]; - cur_rte = rte; + /* compute all distances */ + /* sort XTE array, lowest XTE last */ + Waypoint* pwpt = nullptr; + Waypoint* ppwpt = nullptr; + WaypointList::size_type pos = 0; + neighborhood nb; + trackerror te; + QMap errormap; + QHash wpthash; + for (auto* wpt : rte->waypoint_list) { + wpt->extra_data = nullptr; + + if (metric == metric_t::relative) { + // check hdop is available for compute_track_error + if (wpt->hdop == 0) { + fatal(MYNAME ": relative needs hdop information.\n"); + } + } -} + if (pwpt != nullptr) { + nb.wpt = pwpt; + nb.prev = ppwpt; + nb.next = wpt; -void SimplifyRouteFilter::shuffle_xte(struct xte* xte_rec) -{ - while (xte_rec > xte_recs && compare_xte(xte_rec, xte_rec-1) < 0) { - std::swap(xte_rec[0], xte_rec[-1]); - xte_rec[0].intermed->xte_rec = &xte_rec[0]; - xte_rec[-1].intermed->xte_rec = &xte_rec[-1]; - xte_rec--; - } - while (xte_rec - xte_recs < xte_count-2 && - compare_xte(xte_rec, xte_rec+1) > 0) { - std::swap(xte_rec[0], xte_rec[1]); - xte_rec[0].intermed->xte_rec = &xte_rec[0]; - xte_rec[1].intermed->xte_rec = &xte_rec[1]; - xte_rec++; - } -} + te.dist = compute_track_error(nb); + te.wptpos = pos; -void SimplifyRouteFilter::routesimple_tail(const route_head* rte) -{ - if (!cur_rte) { - return; - } + errormap.insert(te, nb); + wpthash.insert(pwpt, te); + } - /* compute all distances */ - for (int i = 0; i < xte_count ; i++) { - compute_xte(xte_recs+i); + ppwpt = pwpt; + pwpt = wpt; + ++pos; } + nb.wpt = pwpt; + nb.prev = ppwpt; + nb.next = nullptr; + te.dist = compute_track_error(nb); + te.wptpos = pos; - /* sort XTE array, lowest XTE last */ - auto compare_xte_lambda = [](const xte& a, const xte& b)->bool { - return compare_xte(&a, &b) < 0; - }; - if (gpsbabel_testmode()) { - std::stable_sort(xte_recs, xte_recs + xte_count, compare_xte_lambda); - } else { - std::sort(xte_recs, xte_recs + xte_count, compare_xte_lambda); - } + errormap.insert(te, nb); + wpthash.insert(pwpt, te); - - for (int i = 0; i < xte_count; i++) { - xte_recs[i].intermed->xte_rec = xte_recs+i; - } - // Ensure totalerror starts with the distance between first and second points - // and not the zero-init. From a June 25, 2014 thread titled "Simplify - // Filter: GPSBabel removes one trackpoint..." I never could repro it it - // with the sample data, so there is no automated test case, but Steve's - // fix is "obviously" right here. - if (xte_count >= 1) { - totalerror = xte_recs[xte_count-1].distance; - } + assert(!errormap.isEmpty()); + double totalerror = errormap.lastKey().dist; /* while we still have too many records... */ - while ((xte_count) && - (((limit_basis == limit_basis_t::count) && (count < xte_count)) || + while ((!errormap.isEmpty()) && + (((limit_basis == limit_basis_t::count) && (count < errormap.size())) || ((limit_basis == limit_basis_t::error) && (totalerror < error)))) { - int i = xte_count - 1; + /* remove the record with the lowest XTE */ - (*waypt_del_fnp)(const_cast(rte), - const_cast(xte_recs[i].intermed->wpt)); - delete xte_recs[i].intermed->wpt; - - if (xte_recs[i].intermed->prev) { - xte_recs[i].intermed->prev->next = xte_recs[i].intermed->next; - compute_xte(xte_recs[i].intermed->prev->xte_rec); - shuffle_xte(xte_recs[i].intermed->prev->xte_rec); + neighborhood goner = errormap.last(); + goner.wpt->extra_data = &delete_flag; // mark for deletion + // errormap.remove(lastKey()); // with Qt 5.12.12, 5.15.2 results in asan heap-use-after-free errors in build_extra_tests.sh + errormap.erase(--errormap.end()); // in Qt6 can use cend(). + // wpthash.remove(goner.wpt); // removal not necessary + + /* recompute neighbors of point marked for deletion. */ + if (goner.prev != nullptr) { + assert(wpthash.contains(goner.prev)); + trackerror terr = wpthash.value(goner.prev); + assert(errormap.contains(terr)); + neighborhood nbh = errormap.value(terr); + nbh.next = goner.next; + errormap.remove(terr); + terr.dist = compute_track_error(nbh); + errormap.insert(terr, nbh); + wpthash.insert(goner.prev, terr); } - if (xte_recs[i].intermed->next) { - xte_recs[i].intermed->next->prev = xte_recs[i].intermed->prev; - compute_xte(xte_recs[i].intermed->next->xte_rec); - shuffle_xte(xte_recs[i].intermed->next->xte_rec); + if (goner.next != nullptr) { + assert(wpthash.contains(goner.next)); + trackerror terr = wpthash.value(goner.next); + assert(errormap.contains(terr)); + neighborhood nbh = errormap.value(terr); + nbh.prev = goner.prev; + errormap.remove(terr); + terr.dist = compute_track_error(nbh); + errormap.insert(terr, nbh); + wpthash.insert(goner.next, terr); } - xte_count--; - free_xte(xte_recs+xte_count); /* compute impact of deleting next point */ - if (xte_count) { - if (limit_basis == limit_basis_t::error) { - i = xte_count - 1; - switch (metric) { - case metric_t::crosstrack: - case metric_t::relative: - totalerror = xte_recs[i].distance; - break; - case metric_t::length: - totalerror += xte_recs[i].distance; - break; - } + if (limit_basis == limit_basis_t::error) { + switch (metric) { + case metric_t::crosstrack: + case metric_t::relative: + totalerror = errormap.lastKey().dist; + break; + case metric_t::length: + totalerror += errormap.lastKey().dist; + break; + } + } + + } /* end of too many records loop */ + + /* delete marked points */ + // For lineary complexity build a new list from the points we keep. + WaypointList oldlist; + (*route_swap_wpts_fnp)(const_cast(rte), oldlist); + + // mimic trkseg handling from WaypointList::del_rte_waypt + bool inherit_new_trkseg = false; + for (Waypoint* wpt : qAsConst(oldlist)) { + if (wpt->extra_data == nullptr) { + if (inherit_new_trkseg) { + wpt->wpt_flags.new_trkseg = 1; + inherit_new_trkseg = false; + } + (*route_add_wpt_fnp)(const_cast(rte), wpt, u"RPT", 3); + } else { + if (wpt->wpt_flags.new_trkseg) { + inherit_new_trkseg = true; } + delete wpt; } - } /* end of loop */ - if (xte_count) { - do { - xte_count--; - free_xte(xte_recs+xte_count); - } while (xte_count); } - delete[] xte_recs; } void SimplifyRouteFilter::process() { - WayptFunctor routesimple_waypt_pr_f(this, &SimplifyRouteFilter::routesimple_waypt_pr); - RteHdFunctor routesimple_head_f(this, &SimplifyRouteFilter::routesimple_head); RteHdFunctor routesimple_tail_f(this, &SimplifyRouteFilter::routesimple_tail); - waypt_del_fnp = route_del_wpt; - route_disp_all(routesimple_head_f, routesimple_tail_f, routesimple_waypt_pr_f); + route_add_wpt_fnp = route_add_wpt; + route_swap_wpts_fnp = route_swap_wpts; + route_disp_all(nullptr, routesimple_tail_f, nullptr); - waypt_del_fnp = track_del_wpt; - track_disp_all(routesimple_head_f, routesimple_tail_f, routesimple_waypt_pr_f); + route_add_wpt_fnp = track_add_wpt; + route_swap_wpts_fnp = track_swap_wpts; + track_disp_all(nullptr, routesimple_tail_f, nullptr); } void SimplifyRouteFilter::init() diff --git a/smplrout.h b/smplrout.h index d847371d2..7c8e0ed7c 100644 --- a/smplrout.h +++ b/smplrout.h @@ -1,7 +1,7 @@ /* Route / track simplification filter - Copyright (C) 2002-2014 Robert Lipe, robertlipe+source@gpsbabel.org + Copyright (C) 2002-2023 Robert Lipe, robertlipe+source@gpsbabel.org This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -59,11 +59,12 @@ #ifndef SMPLROUT_H_INCLUDED_ #define SMPLROUT_H_INCLUDED_ -#include // for QString -#include // for QVector +#include // for QString +#include // for QStringView +#include // for QVector -#include "defs.h" // for route_head (ptr only), Waypoint (ptr only), ARGT... -#include "filter.h" // for Filter +#include "defs.h" +#include "filter.h" // for Filter #if FILTERS_ENABLED @@ -72,6 +73,13 @@ class SimplifyRouteFilter:public Filter { public: + /* Types */ + + struct trackerror { + double dist; + WaypointList::size_type wptpos; + }; + /* Member Functions */ QVector* get_args() override @@ -88,26 +96,10 @@ private: enum class limit_basis_t {count, error}; enum class metric_t {crosstrack, length, relative}; - struct xte_intermed; - - struct xte { - double distance{0.0}; - struct xte_intermed* intermed { - nullptr - }; - }; - - struct xte_intermed { - struct xte* xte_rec { - nullptr - }; - struct xte_intermed* next { - nullptr - }; - struct xte_intermed* prev { - nullptr - }; - const Waypoint* wpt{nullptr}; + struct neighborhood { + Waypoint* wpt; + Waypoint* prev; + Waypoint* next; }; /* Constants */ @@ -116,28 +108,24 @@ private: /* Member Functions */ - static void free_xte(struct xte* xte_rec); - void routesimple_waypt_pr(const Waypoint* wpt); - void compute_xte(struct xte* xte_rec); - static int compare_xte(const void* a, const void* b); - void routesimple_head(const route_head* rte); - void shuffle_xte(struct xte* xte_rec); + double compute_track_error(const neighborhood& nb) const; void routesimple_tail(const route_head* rte); /* Data Members */ int count = 0; - double totalerror = 0; double error = 0; limit_basis_t limit_basis{limit_basis_t::error}; metric_t metric{metric_t::crosstrack}; + int delete_flag{}; // &delete_flag != nullptr char* countopt = nullptr; char* erroropt = nullptr; char* xteopt = nullptr; char* lenopt = nullptr; char* relopt = nullptr; - void (*waypt_del_fnp)(route_head* rte, Waypoint* wpt) {}; + void (*route_add_wpt_fnp)(route_head* rte, Waypoint* wpt, QStringView namepart, int number_digits) {}; + void (*route_swap_wpts_fnp)(route_head* rte, WaypointList& other) {}; QVector args = { { @@ -162,10 +150,6 @@ private: }, }; - struct xte_intermed* tmpprev = nullptr; - int xte_count = 0; - const route_head* cur_rte = nullptr; - struct xte* xte_recs = nullptr; }; #endif // FILTERS_ENABLED diff --git a/waypt.cc b/waypt.cc index 39edfccc0..c2c614c35 100644 --- a/waypt.cc +++ b/waypt.cc @@ -393,7 +393,6 @@ Waypoint::Waypoint() : latitude(0), // These should probably use some invalid data, but longitude(0), // it looks like we have code that relies on them being zero. altitude(unknown_alt), - route_priority(0), hdop(0), vdop(0), pdop(0), @@ -435,7 +434,6 @@ Waypoint::Waypoint(const Waypoint& other) : icon_descr(other.icon_descr), creation_time(other.creation_time), wpt_flags(other.wpt_flags), - route_priority(other.route_priority), hdop(other.hdop), vdop(other.vdop), pdop(other.pdop), @@ -485,7 +483,6 @@ Waypoint& Waypoint::operator=(const Waypoint& rhs) wpt_flags = rhs.wpt_flags; icon_descr = rhs.icon_descr; creation_time = rhs.creation_time; - route_priority = rhs.route_priority; hdop = rhs.hdop; vdop = rhs.vdop; pdop = rhs.pdop; -- 2.30.2